#include "IRlocate.h"

IRlocate::IRlocate()
{
	Requires (irsensors);
	Requires (lifter);
	Requires (conveyer);
	Requires (roller);
	state=kEmpty;
	firing = 0;
}
void IRlocate::IRlocateAuton()
{
	state=kAutonAdjust;
}
void IRlocate::Initialize()
{
}
void IRlocate::Execute()
{	
	if(state==kEmpty||state==kBall1||state==kBall2)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
		CommandBase::roller->Forward();
	}
	else if(state==kBall3)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Stop();
		CommandBase::roller->Reverse();
	}
	
	if(irsensors->GetSensor(7))
	{
		if(state==kEmpty)
			state=kLoading1;		
		else if(state==kBall1)
			state=kLoading2;
		else if(state==kBall2)
			state=kLoading3;
	}
	
	if(firing)
	{
		if(state==kBall1)
			state=kFiring1;
		else if(state==kBall2)
			state=kFiring2;
		else if(state==kBall3)
			state=kFiring3;
	}
	
	if(state==kLoading1)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
		CommandBase::roller->Reverse();
		if(irsensors->GetSensor(4) && !irsensors->GetSensor(5) && !irsensors->GetSensor(3))
		{
			state=kBall1;
		}
	}
	else if(state==kLoading2)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
		CommandBase::roller->Reverse();
		if(irsensors->GetSensor(4)&& irsensors->GetSensor(6)&& !irsensors->GetSensor(3)&& !irsensors->GetSensor(5) && !irsensors->GetSensor(7))
			state=kLoading2_2;
	}
	else if(state==kLoading2_2)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
		CommandBase::roller->Reverse();
			if (irsensors->GetSensor(2) && irsensors->GetSensor(4) && !irsensors->GetSensor(3)&& !irsensors->GetSensor(5) )
				state=kBall2;
	}
	else if(state==kLoading3)
	{
		CommandBase::lifter->Stop();
		CommandBase::conveyer->Forward();
		if (irsensors->GetSensor(7))
			state=kBall3;
	}
	else if(state==kFiring1)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if (!irsensors->GetSensor(1) && !irsensors->GetSensor(2) && !irsensors->GetSensor(3) && !irsensors->GetSensor(4) )
			state=kEmpty;
	}
	else if(state==kFiring2)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if(!irsensors->GetSensor(1) && irsensors->GetSensor(2) && !irsensors->GetSensor(3) && !irsensors->GetSensor(4))
			state=kFiring2_2;
	}
	else if(state==kFiring2_2)
	{
		CommandBase::lifter->Reverse();
		CommandBase::conveyer->Stop();
		CommandBase::roller->Reverse();
		if(!irsensors->GetSensor(1) && !irsensors->GetSensor(2) && !irsensors->GetSensor(3) && irsensors->GetSensor(4))
			state=kBall1;
	}
	else if(state==kFiring3)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Stop();
		if(!irsensors->GetSensor(1) && irsensors->GetSensor(2) && !irsensors->GetSensor(3) && !irsensors->GetSensor(4) && !irsensors->GetSensor(5) && !irsensors->GetSensor(6) && irsensors->GetSensor(7))
			state=kFiring3_2;
	}
	else if(state==kFiring3_2)
	{
		CommandBase::lifter->Reverse();
		CommandBase::conveyer->Stop();
		CommandBase::roller->Reverse();
		if(!irsensors->GetSensor(1) && !irsensors->GetSensor(2) && !irsensors->GetSensor(3) && !irsensors->GetSensor(4)&& !irsensors->GetSensor(5) && !irsensors->GetSensor(6) && !irsensors->GetSensor(7))
			state=kFiring3_3;
	}
	else if(state==kFiring3_3)
	{
		CommandBase::lifter->Forward();
		CommandBase::conveyer->Forward();
		CommandBase::roller->Reverse();
		if(!irsensors->GetSensor(1) && irsensors->GetSensor(2) && !irsensors->GetSensor(3) && irsensors->GetSensor(4) && irsensors->GetSensor(5) && !irsensors->GetSensor(6) && irsensors->GetSensor(7))
			state=kBall2;
	}
	if (state==kAutonAdjust)
	{
		CommandBase::lifter->Reverse(); // balls start one on roller, and one in  lifter
		CommandBase::conveyer->Stop();
		CommandBase::roller->Stop();
		if(irsensors->GetSensor(4) && !irsensors->GetSensor(3) && !irsensors->GetSensor(5) && !irsensors->GetSensor(2) && !irsensors->GetSensor(1))
		{
			CommandBase::roller->Forward();
			CommandBase::lifter->Forward();
			CommandBase::conveyer->Forward();
			state=kAutonFiring;
			firing=1;
		}
	}
}
bool IRlocate::IsFinished()
{
	return 0;
}
void IRlocate::End()
{
	
}
void IRlocate::Interrupted()
{
	
}

int IRlocate::GetState()
{
	return int(state);
}
